
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       AccelCalibrator.h
  * @author     baiyang
  * @date       2022-3-13
  ******************************************************************************
  */

#pragma once

#ifdef __cplusplus
extern "C"{
#endif

/*----------------------------------include-----------------------------------*/
#include <stdint.h>
#include <stdbool.h>

#include <common/gp_math/gp_mathlib.h>
/*-----------------------------------macro------------------------------------*/
#define ACCEL_CAL_MAX_NUM_PARAMS 9
#define ACCEL_CAL_TOLERANCE 0.1f
#define MAX_ITERATIONS  50
/*----------------------------------typedef-----------------------------------*/
enum accel_cal_status_t {
    ACCEL_CAL_NOT_STARTED=0,
    ACCEL_CAL_WAITING_FOR_ORIENTATION=1,
    ACCEL_CAL_COLLECTING_SAMPLE=2,
    ACCEL_CAL_SUCCESS=3,
    ACCEL_CAL_FAILED=4
};

enum accel_cal_fit_type_t {
    ACCEL_CAL_AXIS_ALIGNED_ELLIPSOID=0,
    ACCEL_CAL_ELLIPSOID=1
};

struct accel_cal_param_t {
    Vector3f_t offset;
    Vector3f_t diag;
    Vector3f_t offdiag;
};

struct AccelSample {
    Vector3f_t delta_velocity;
    float delta_time;
};

union accel_cal_param_u {
    struct accel_cal_param_t s;
    float a[ACCEL_CAL_MAX_NUM_PARAMS];
};

typedef  float VectorP[ACCEL_CAL_MAX_NUM_PARAMS];
typedef  struct accel_calibrator* accelcal_t;

/** @ 
  * @brief  
  */
struct accel_calibrator {
    //configuration
    uint8_t _conf_num_samples;
    float _conf_sample_time;
    enum accel_cal_fit_type_t _conf_fit_type;
    float _conf_tolerance;

    // state
    enum accel_cal_status_t _status;
    struct AccelSample* _sample_buffer;
    uint8_t _samples_collected;
    union accel_cal_param_u _param;
    float _fitness;
    uint32_t _last_samp_frag_collected_ms;
    float _min_sample_dist;
};
/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
void accelcal_ctor(accelcal_t accelcal);
void accelcal_clear(accelcal_t accelcal);
void accelcal_start(accelcal_t accelcal, enum accel_cal_fit_type_t fit_type, uint8_t num_samples, float sample_time);
void accelcal_start2(accelcal_t accelcal, enum accel_cal_fit_type_t fit_type, uint8_t num_samples, float sample_time, Vector3f_t* offset, Vector3f_t* diag, Vector3f_t* offdiag);
bool accelcal_running(accelcal_t accelcal);
void accelcal_collect_sample(accelcal_t accelcal);
void accelcal_new_sample(accelcal_t accelcal, const Vector3f_t* delta_velocity, float dt);
bool accelcal_get_sample(accelcal_t accelcal, uint8_t i, Vector3f_t* s);
bool accelcal_get_sample_corrected(accelcal_t accelcal, uint8_t i, Vector3f_t* s);
void accelcal_check_for_timeout(accelcal_t accelcal);
void accelcal_get_calibration(accelcal_t accelcal, Vector3f_t* offset);
void accelcal_get_calibration2(accelcal_t accelcal, Vector3f_t* offset, Vector3f_t* diag);
void accelcal_get_calibration3(accelcal_t accelcal, Vector3f_t* offset, Vector3f_t* diag, Vector3f_t* offdiag);

// set tolerance bar for parameter fitness value to cross so as to be deemed as correct values
static inline void accelcal_set_tolerance(accelcal_t accelcal, float tolerance) { accelcal->_conf_tolerance = tolerance; }

// returns current state of accel calibrators
static inline enum accel_cal_status_t accelcal_get_status(accelcal_t accelcal) { return accelcal->_status; }

// returns number of samples collected
static inline uint8_t accelcal_get_num_samples_collected(accelcal_t accelcal) { return accelcal->_samples_collected; }

// returns mean squared fitness of sample points to the selected surface
static inline float accelcal_get_fitness(accelcal_t accelcal) { return accelcal->_fitness; }
/*------------------------------------test------------------------------------*/

#ifdef __cplusplus
}
#endif



